/*
 * BamFrameSet.cpp
 *
 *  Created on: Oct 2, 2017
 *      Author: famez
 */

#include <string.h>

//Common
#include <Utils.h>
#include <Assert.h>

//J1939
#include <J1939Factory.h>
#include <Transport/BAM/BamReassembler.h>

namespace J1939 {

u8 BamReassembler::BAMFragments::getLastSQ() const{

	if(mDTFrames.empty())
	{
		return 0;
	}

	return mDTFrames.back().getSq();

}



BamReassembler::BamReassembler() : mLastError(BAM_ERROR_OK) {

}

BamReassembler::~BamReassembler() {
	clear();
}

void BamReassembler::reassemble(const BAMFragments& fragments, u8** data, size_t& length) {

	size_t offset = 0;

	length = fragments.getCmFrame().getTotalMsgSize();
	*data = new u8[length];

	for(std::vector<TPDTFrame>::const_iterator iter = fragments.getDtFrames().begin(); iter != fragments.getDtFrames().end(); iter++) {
		memcpy((void*)(*data + offset), (void*)iter->getData(), J1939_MIN(length - offset, TP_DT_PACKET_SIZE));
		offset += TP_DT_PACKET_SIZE;
		if(offset >= length)
		{
			break;
		}
	}
}

bool BamReassembler::toBeHandled(const J1939Frame& frame) const {

	const TPCMFrame* conn;

	if(frame.getDstAddr() != J1939_BROADCAST_ADDRESS) {		//Does not have a broadcast address
		return false;
	}

	switch(frame.getPGN()) {
	case TP_CM_PGN:
	{
		conn = static_cast<const TPCMFrame*>(&frame);
		return (conn->getCtrlType() == CTRL_TPCM_BAM);		//If the ctrl type is not BAM, discard it.
	}	break;
	case TP_DT_PGN:
	{
		return true;
	}	break;
	default:
	{
		return false;
	}	break;

	}

}


size_t BamReassembler::handleFrame(const J1939Frame& frame) {

	const TPDTFrame* data = NULL;
	const TPCMFrame* conn = NULL;

	size_t expectedSize = 0;

	u8 srcAddr = frame.getSrcAddr();


	if(frame.getDstAddr() != J1939_BROADCAST_ADDRESS) {		//The frame does not have a broadcast address
		setError(BAM_ERROR_NOT_BCAST_ADDR);
		return 0;
	}

	switch(frame.getPGN()) {
	case TP_CM_PGN:					//Conn management reception
	{
		conn = static_cast<const TPCMFrame*>(&frame);

		//Not the right Connection Manager frame...
		if(conn->getCtrlType() != CTRL_TPCM_BAM) {

			setError(BAM_ERROR_UNEXPECTED_FRAME);
			break;
		}

		//New TP.CM frame but not all previous TP.DT frames were received
		if(mFragments.find(srcAddr) != mFragments.end()) {
			mFragments.erase(srcAddr);
			setError(BAM_ERROR_INCOMPLETE_FRAME);
		} else {
			setError(BAM_ERROR_OK);
		}

		mFragments[srcAddr].setCmFrame(*conn);

		expectedSize = conn->getTotalMsgSize();

	}	break;

	case TP_DT_PGN:					//Data reception
	{

		if(mFragments.find(srcAddr) == mFragments.end()) {
			setError(BAM_ERROR_UNEXPECTED_FRAME);
			break;
		}

		data = static_cast<const TPDTFrame*>(&frame);

		u8 sq = data->getSq();

		const TPCMFrame& connFrame = mFragments[srcAddr].getCmFrame();

		expectedSize = connFrame.getTotalMsgSize();

		if((connFrame.getTotalPackets() >= sq) && (mFragments[srcAddr].getLastSQ() + 1 == sq) ) {
			mFragments[srcAddr].addDtFrame(*data);
		} else {
			setError(BAM_ERROR_UNEXPECTED_FRAME);
			break;
		}

		if(sq == connFrame.getTotalPackets()) {		//Time to reassemble the packet

			u8* fullData;
			size_t length;
			reassemble(mFragments[srcAddr], &fullData, length);
			u32 newId = (((connFrame.getPriority() & J1939_PRIORITY_MASK) << J1939_PRIORITY_OFFSET) |
					(connFrame.getDataPgn() << J1939_PGN_OFFSET) | (connFrame.getSrcAddr() & J1939_SRC_ADDR_MASK));

			std::unique_ptr<J1939Frame> decodedFrame = J1939Factory::getInstance().getJ1939Frame(newId, fullData, length);
			delete[] fullData;

			//Fragments reassembled, we can destroy the fragments
			mFragments.erase(srcAddr);

			if(decodedFrame.get()) {
				mReassembledFrames.push(decodedFrame.release());
			} else {
				setError(BAM_ERROR_DECODING);
				return 0;
			}

			setError(BAM_ERROR_OK);
		}

	}	break;

	default:
	{
		//Not a frame for BAM protocol
		setError(BAM_ERROR_UNEXPECTED_FRAME);

	}	break;

	}

	return expectedSize;

}


std::unique_ptr<J1939Frame> BamReassembler::dequeueReassembledFrame() {

	J1939Frame* retVal = NULL;

	retVal = mReassembledFrames.front();
	mReassembledFrames.pop();


	return std::unique_ptr<J1939Frame>(retVal);

}

void BamReassembler::clear() {

	mFragments.clear();

	mLastError = BAM_ERROR_OK;

	//Clear queue
	while(!mReassembledFrames.empty()) {
		delete mReassembledFrames.front();
		mReassembledFrames.pop();
	}

}

} /* namespace J1939 */
